package robot.calibeta;

import java.util.ArrayList;
import java.util.Random;
import java.util.Stack;

import chair.Organ;
import chair.core.Diary;

/**
 * Cette classe gère la connaissance spatiale du robot. La position de sa niche,
 * celles des différentes pièces qu'il aura trouvé, etc.
 * 
 * @author nomhad
 * 
 */
public class Cartographer {

	/**
	 * Les différentes stratégies que peut adopter le cartographe pour tracer
	 * l'itinéraire. Appelé par les comportements et réflexes reliés à la
	 * fonction "cartographe".
	 */
	public static enum Route {
		/**
		 * Explorer une branche inconnue. reachedDestination() quand plus aucune
		 * branche inconnue ou quand a atteint une nouvelle salle.
		 */
		EXPLORE,
		/** Retour à la niche */
		KENNEL,
		/** Se rendre à la salle particulière */
		ROOM;
	};

	/** On est très orthogonal dans le coin */
	public static enum Cape {
		NORTH(0), SOUTH(180), EAST(-90), WEST(90);
		/** Angle par rapport au Nord */
		private float angle;

		Cape(float ang) {
			angle = ang;
		}

		/**
		 * À quel angle par rapport au Nord correspond ce cap ?
		 * 
		 * @return angle
		 */
		public float getAngle() {
			return angle;
		}

		/**
		 * Retourne le cap à l'opposé de celui passé en paramètre. Utile pour
		 * relier deux carrefour entre eux.
		 * 
		 * @return cap situé à 180°
		 */
		public Cape opposite() {
			// Calcule l'angle correspondant au cap opposé
			float oppositeAngle = normalizeAngle(angle + 180);
			// Parcours valeurs pour trouver correspondance
			for (Cape cap : Cape.values()) {
				if (cap.angle == oppositeAngle)
					return cap;
			}
			throw new IllegalStateException(
					"Impossible de trouver le cap opposé");
		}

		/**
		 * Retourne le cap situé à 90° de celui passé en paramètre. Utilisé dans
		 * discoverJunction() et discoverRoom().
		 * 
		 * @return cap adjacent
		 */
		public Cape adjacent() {
			// Calcule l'angle correspondant au cap adjacent
			float oppositeAngle = normalizeAngle(angle + 90);
			// Parcours valeurs pour trouver correspondance
			for (Cape cap : Cape.values()) {
				if (cap.angle == oppositeAngle)
					return cap;
			}
			throw new IllegalStateException(
					"Impossible de trouver le cap adjavent");
		}
	};

	/**
	 * Junction peut recouper plusieurs réalités.
	 */
	public static enum JunctionType {
		/** la niche, autrement dit le point de départ du robot */
		KENNEL,
		/** carrefour au sens véritable */
		NODE,
		/** un point d'arrivé du robot */
		ROOM
	};

	/**
	 * Quelle est la couleur présente sous le robot ? Pour remplir contrat doit
	 * signaler par 1 du hors piste et avec une valeur >= 3 une salle
	 */
	private Organ _seeColor;

	/**
	 * combien de ms le robot s'arrête lors de l'exploration de chaque carrefour
	 * pour attendre la lecture d'une couleur
	 */
	private final long TIME_ON_EACH_BRANCH = 500;

	/** Itinéraire actuellement utilisé par Cartographer */
	private Route _currentRoute = Route.EXPLORE;
	/** Dans le cas de Route.ROOM, le nom associé */
	private String _currentRouteRoomName = "";
	/** Est-ce que le robot a atteint sa destination actuelle */
	private boolean _destinationReached = false;
	/**
	 * Est-ce que le robot se préoccupe des branches EAST/WEST lors de la
	 * découverte des carrefours ?
	 * 
	 * FIXME: rustine en vitesse, figé pour les 4 cap actuels
	 */
	private boolean _watchSides = true;
	/**
	 * Utile de savoir si robot a découvert son premier point d'attache, on le
	 * fait alors avancer
	 */
	private boolean _hasDiscoveredKennel = false;

	/**
	 * Quels sont les chemins qui partent d'un carrefour. Attention, défini
	 * suivant orientation du robot lors du premier passage. Un identifiant
	 * s'incrémente à chaque construction : impossible de créer deux carrefours
	 * identiques.
	 * 
	 * @author nomhad
	 * 
	 */
	private static class Junction {
		/** Ou mène chaque branche ? */
		private Junction[] junctions;
		/** Quelle branche doit-on explorer en ce moment ? */
		private Cape selectedCape;
		/** quels sont les banches qui existent */
		private boolean[] branches;
		/*** quelles sont les banches visitées */
		private boolean[] branchesVisited;
		/** le type du carrefour */
		private JunctionType type;
		/** Incrémenté pour ID par défaut */
		private static int count = 0;
		/** identifiant du carrefour */
		private final String id;
		/** nombre de branches valides que possède ce carrefour */
		private int nbBranches = 0;
		/** nombre de branches valides inexplorées */
		private int nbWildBranches = 0;

		Junction(JunctionType type) {
			this(type, "" + count++);
		}

		Junction(JunctionType type, String id) {
			this.type = type;
			this.id = id;
			junctions = new Junction[Cape.values().length];
			branches = new boolean[Cape.values().length];
			branchesVisited = new boolean[Cape.values().length];
			// C'est triste mais par le carrefour née dans la pampa
			for (int i = 0; i < branches.length; i++) {
				branches[i] = false;
				branchesVisited[i] = false;
			}
		}

		/**
		 * Retourne identifiant, utilisé pour savoir si retrouve bonne salle
		 * dans onRoom().
		 * 
		 * @return id
		 */
		String getId() {
			return id;
		}

		/**
		 * Indique à quel autre carrefour mène telle branche (déclare la branche
		 * en même temps).
		 * 
		 * @param cap
		 *            cap emprunté
		 * @param junction
		 *            carrefour de destination
		 */
		synchronized void setJunction(Cape cap, Junction junction) {
			// Relie carrefour avant d'appleler prochaine méthode car cette
			// dernière tient les comptes
			junctions[cap.ordinal()] = junction;
			// Si on nous indique qu'une branche mène à un carrefour, c'est
			// qu'elle existe. Confiance aveugle.
			setBranche(cap, true);
		}

		/**
		 * On indique si le carrefour a un chemin qui part dans cette direction
		 * 
		 * @param cap
		 *            cap de la branche
		 * @param toggle
		 *            branche existe ou non
		 */
		synchronized void setBranche(Cape cap, boolean toggle) {
			Diary.logln(toString() + " vers " + cap + " la branche est "
					+ toggle);
			branches[cap.ordinal()] = toggle;
			branchesVisited[cap.ordinal()] = true;
			// Tient les comptes des branches à jour
			nbBranches = 0;
			nbWildBranches = 0;
			for (int i = 0; i < branches.length; i++) {
				if (branches[i]) {
					nbBranches++;
					if (junctions[i] == null)
						nbWildBranches++;
				}
			}
		}

		/**
		 * À chaque carrefour le robot prend une décision. Vérifie que la
		 * branche bien été auparavant visitée et existe.
		 * 
		 * @param cap
		 *            Quelle est le cap emprunté à ce carrefour ? Si null, RAZ
		 *            sans plus de vérifications.
		 */
		void selectCape(Cape cap) {
			// computeRoute() cherche à faire des RAZ, vérifie rien d'autre
			// avant de retourner
			if (cap == null) {
				selectedCape = cap;
				return;
			}
			int i = cap.ordinal();
			if (!branches[i])
				throw new IllegalArgumentException(
						"Ce carrefour ne possède pas cette branche");
			if (!branchesVisited[i])
				throw new IllegalArgumentException(
						"Cette branche n'a pas été visitée.");
			selectedCape = cap;
		}

		/**
		 * Quelle branche a-t-on explorée à ce carrefour ?
		 * 
		 * @return "D'où venons-nous" : question universelle.
		 */
		Cape getSelectedCape() {
			return selectedCape;
		}

		/**
		 * À quel carrefour mène la branche ici sélectionnée ?
		 * 
		 * @return carrefour au bout de selectedDirection
		 */
		Junction getSelectedJunction() {
			if (selectedCape != null)
				return junctions[selectedCape.ordinal()];
			else
				Diary.logln("Problème : aucun cap défini pour ce carrefour");
			return null;
		}

		/**
		 * À quel carrefour mène ce cap ?
		 * 
		 * @param cape
		 *            cap dont on veut connaître l'issu
		 * @return carrefour au bout de ce cap
		 */
		Junction getJunction(Cape cape) {
			return junctions[cape.ordinal()];
		}

		/**
		 * Renseigne sur la nature du carrefour
		 * 
		 * @return type du carrefour
		 */
		JunctionType getType() {
			return type;
		}

		/**
		 * Quelles sont les branches qui restent encore à explorer ?
		 * 
		 * @return tableau contenant les directions aux issues inconnues
		 */
		synchronized Cape[] getWildBranches() {
			// Combien de branches inexplorées ?
			int nbWild = nbWildBranches;
			// Les directions correspondant à ces branches
			Cape[] capes = new Cape[nbWild];
			// Ne reste plus qu'à les trouver
			for (int i = 0; i < branches.length && nbWild > 0; i++) {
				if (branches[i])
					if (junctions[i] == null)
						// Ça pique : utilise nbWild comme compteur pour savoir
						// combien de directions à connaître et récupère indice
						// correspondant dans l'enum des directions
						capes[--nbWild] = Cape.values()[i];
			}
			return capes;
		}

		/**
		 * Quelles sont les branches explorées ?
		 * 
		 * @return tableau contenant les directions aux issues connues
		 */
		synchronized Cape[] getKnownBranches() {
			// Combien de branches inexplorées ?
			int nbKnown = nbBranches - nbWildBranches;
			// Les directions correspondant à ces branches
			Cape[] capes = new Cape[nbKnown];
			// Ne reste plus qu'à les trouver
			for (int i = 0; i < branches.length && nbKnown > 0; i++) {
				if (branches[i])
					if (junctions[i] != null)
						capes[--nbKnown] = Cape.values()[i];
			}
			return capes;
		}

		/**
		 * Combien de branche inexplorées ce carrefour compte-il ?
		 * 
		 * @return nombre de branche existantes menant à une destination
		 *         inconnue
		 */
		synchronized int getNbWildBranches() {
			return nbWildBranches;
		}

		/**
		 * Combien de branche ce carrefour compte-il ?
		 * 
		 * @return nombre de branches existantes pour le moment
		 */
		synchronized int getNbBranches() {
			return nbBranches;
		}

		@Override
		public int hashCode() {
			final int prime = 17;
			int result = 1;
			result = prime * result + ((id == null) ? 0 : id.hashCode());
			result = prime * result + ((type == null) ? 0 : type.hashCode());
			return result;
		}

		@Override
		public boolean equals(Object obj) {
			if (obj instanceof Junction) {
				Junction other = (Junction) obj;
				return id.equals(other.id) && type == other.type;
			}
			return false;
		}

		@Override
		public String toString() {
			return "Junction [id=" + id + ", type=" + type + "]";
		}
	}

	/**
	 * Un chemin composé d'une succession de carrefours. Si les carrefours sont
	 * créés et connectés par Cartographer c'est ici qu'on défini les
	 * itinéraires.
	 * 
	 * @author nomhad
	 * 
	 */
	private class Map {
		/** Tous les carrefours de la carte */
		private ArrayList<Junction> nodes;
		/** Dernier carrefour rencontré */
		private Junction lastJunction = null;
		/** Utilisé pour sélectionner branche au hasard */
		private Random random = new Random();
		/**
		 * Nombre de carrefour avec des branches qui restent à explorer. Servira
		 * à BehaviorExplore via hasWildJunctions pour savoir si doit se
		 * déclarer non pertinent.
		 */
		private int nbWildJunctions = -1;

		Map() {
			nodes = new ArrayList<Junction>();
		}

		/** Retourne le carrefour d'où vient le robot */
		Junction getLastJunction() {
			return lastJunction;
		}

		/**
		 * Va sélectionner à ce carrefour une des banches inexplorées au hasard.
		 * 
		 * @param jun
		 *            carrefour dont il faut définir l'itinéraire
		 */
		private void selectRandomBranch(Junction jun) {
			// On va regarder quelles sont les branches inexplorées
			Cape[] cap = jun.getWildBranches();
			// Aucune branche : mauvais appel
			if (cap.length == 0)
				throw new IllegalStateException(
						"Impossible de trouver branches sauvages");
			// Le condensé : ça me connaît. Tire au hasard une des cases du
			// tableau et la donne comme direction au carrefour.
			jun.selectCape(cap[random.nextInt(cap.length)]);
		}

		/**
		 * Met à jour nbWildJunctions : parcourt les carrefours et comptabilise
		 * ceux qui ont des branches inexplorés. Appelé par à chaque rencontre
		 * de nœuuds via setJunction
		 */
		private void computeWildJunctions() {
			nbWildJunctions = 0;
			synchronized (nodes) {
				for (Junction node : nodes) {
					if (node.getNbWildBranches() > 0)
						nbWildJunctions++;
				}
			}
			Diary.logln("Nombre de carrefours sauvages : " + nbWildJunctions);
		}

		/**
		 * Un nouveau carrefour rencontré : on est en mode EXPLORE, ajoute à
		 * liste et on en profite pour définir quel chemin on va explorer
		 * (prochaine branche au hasard ou marche arrière si cul-de-sac et
		 * recalcule). Note : ce nouveau carrefour a déjà été relié à un
		 * précédent nœud par les bons soin de Cartographer.onJunction().
		 * 
		 * @param jun
		 *            nouveau carrefour à ajouter à la carte et dont il faut
		 *            définir quelle route prendre
		 */
		private void addJunction(Junction jun) {
			if (_currentRoute != Route.EXPLORE)
				throw new IllegalStateException(
						"Impossible de rencontrer de nouveaux carrefour le robot n'est pas en mode d'exploration");
			synchronized (nodes) {
				nodes.add(jun);
			}
			// Met à jour nbWildJunctions
			computeWildJunctions();
			// Si on viens d'ajouter une salle on laisse gérer plus haut le
			// nouvel itinéraire
			if (jun.getType() == JunctionType.ROOM)
				return;
			// On ajouté un cul-de-sac, il faut recalculer tout l'itinéraire
			// NB: niche cas spécial, on va pas lui faire faire demi-tour
			if (jun.getNbBranches() <= 1
					&& jun.getType() != JunctionType.KENNEL) {
				Diary.logln("Tombe sur cul-de-sac, recalcule itinéraire");
				computeRoute();
			} else
				// Sinon on continue exploration
				selectRandomBranch(jun);
		}

		/**
		 * On est passé sur un carrefour, on tient la carte à jour et on regarde
		 * si on a atteint notre destination.
		 * 
		 * @param jun
		 *            dernier carrefour sur lequel est passé le robot
		 */
		void setJunction(Junction jun) {
			lastJunction = jun;
			// Ce carrefour est nouveau : on l'ajoute à la liste, itinéraire
			// EXPLORE poursuivi
			synchronized (nodes) {
				if (!nodes.contains(jun)) {
					addJunction(jun);
				}
				// Dans le cas de l'exploration, on a intérêt à garder à jour
				// nbWildJunctions (addJunction s'en ai occupé de son côté car
				// il y a mise à jour de "nodes")
				else if (_currentRoute == Route.EXPLORE)
					computeWildJunctions();
			}

			// On vérifie si on a pas rempli un objectif
			switch (_currentRoute) {
			case KENNEL:
				if (jun.getType() == JunctionType.KENNEL) {
					Diary.logln("Retour à la niche effectué");
					setDestinationReached(true);
				}
				break;
			case ROOM:
				if (jun.getType() == JunctionType.ROOM) {
					if (jun.getId().equals(_currentRouteRoomName)) {
						Diary.logln("Arrivé à la bonne salle");
						setDestinationReached(true);
					} else
						// Pas d'exception au cas où je veuille un jour salle
						// pas un cul-de-sac
						Diary.logln("Problème : mauvaise salle");
				}
				break;
			case EXPLORE:
				if (jun.getType() == JunctionType.ROOM) {
					Diary
							.logln("Explore se termine ici par la découverte d'une salle.");
					setDestinationReached(true);
				}
				break;
			}
		}

		/**
		 * Retourne le prochain cap à prendre.
		 * 
		 * @return cap sélectionné par le dernier carrefour
		 */
		Cape getNextCape() {
			if (lastJunction == null) {
				return null;
			}
			return lastJunction.getSelectedCape();
		}

		/**
		 * Retourne le carrefour vers lequel le robot est sensé se diriger.
		 * 
		 * @return carrefour au bout de la branche qu'a emprunté le robot
		 */
		Junction getNextJunction() {
			if (lastJunction != null) {
				return lastJunction.getSelectedJunction();
			}
			return null;
		}

		/**
		 * Suivant le type d'itinéraire demandé et ce qu'il reste à découvrir,
		 * va vérifier ce qui peut être fait et va ordonner calcul d'un
		 * itinéraire le cas échéant. Lève quelques exception si impossible et
		 * déclare un objectif atteint si plus rien à explorer. Retourne si pas
		 * n'a même pas encore trouvé sa niche.
		 */
		private void computeRoute() {
			// On nous a demandé nouvel itinéraire : nouvelle aventure
			setDestinationReached(false);
			// Le nœud qu'on cherche à joindre
			Junction nodeAim = null;

			switch (_currentRoute) {
			// Mode exploration : amène au hasard vers un carrefour
			// inexploré et sélectionne au hasard une branche inexploré
			case EXPLORE:
				// Aucun carrefour sauvage trouvé, peut considéré qu'on a
				// remplit contrat.
				if (nbWildJunctions == 0) {
					Diary
							.logln("computeRoute() Route.EXPLORE terminé, plus aucun carrefour avec branches sauvages trouvé.");
					setDestinationReached(true);
					return;
				}

				Junction[] wildJunctions;
				synchronized (nodes) {
					// Robot attend toujours de rencontrer sa niche, on le
					// laisse avancer
					if (nodes.size() == 0) {
						Diary
								.logln("computeRoute() Rien de cartographié, rien à calculer.");
						return;
					}
					// Sinon on commence les calculs
					wildJunctions = new Junction[nbWildJunctions];
					int n = 0;
					for (Junction node : nodes) {
						if (node.getNbWildBranches() > 0)
							wildJunctions[n++] = node;
					}
				}

				// Un objectif est tiré au sort
				int numAim = random.nextInt(nbWildJunctions);
				nodeAim = wildJunctions[numAim];
				// La sélection de branche s'éffectue plus tard, le calcul de
				// l'itinéraire réinitialise tout
				break;
			// Récupère simplement niche
			case KENNEL:
				synchronized (nodes) {
					for (Junction node : nodes) {
						if (node.getType() == JunctionType.KENNEL)
							nodeAim = node;
					}
				}
				// Impossible de trouver niche
				if (nodeAim == null)
					throw new IllegalStateException("computeRoute()"
							+ _currentRoute + " mais aucune niche existante");
				break;
			// Recherche un salle dont l'id correspond au nom voulu
			case ROOM:
				synchronized (nodes) {
					for (Junction node : nodes) {
						if (node.getType() == JunctionType.ROOM
								&& node.getId().equals(_currentRouteRoomName))
							nodeAim = node;
					}
				}
				// Impossible de trouver salle
				if (nodeAim == null)
					throw new IllegalStateException("computeRoute()"
							+ _currentRoute + "/" + _currentRouteRoomName
							+ " mais aucune salle correspondante trouvée");
				break;
			}
			// Plus qu'à relier position courante à l'objectif
			computeRouteBetween(lastJunction, nodeAim);
			// Dans le cas de l'exploration, il faut encore sélectionner branche
			// au hasard
			if (_currentRoute == Route.EXPLORE)
				selectRandomBranch(nodeAim);
		}

		/**
		 * Va configurer les carrefours pour que le cap qu'il sélectionne pointe
		 * vers la bonne destination.
		 * 
		 * @param nodeFrom
		 *            début du trajet (où se trouve le robot)
		 * @param nodeTo
		 *            fin du trajet (sa destination)
		 */
		// FIXME: Stack pas paramétré sous leJOS, justmement pour éviter un
		// warning de plus que la supression s'applique sur "all" plutôt que sur
		// "unchecked".
		@SuppressWarnings("all")
		private void computeRouteBetween(Junction nodeFrom, Junction nodeTo) {
			// Première chose : RAZ de tous les caps qui ont pu être
			// sélectionnés
			synchronized (nodes) {
				for (Junction node : nodes)
					node.selectCape(null);
			}

			/*
			 * Stratégie de l'algo : empile les carrefours explorés en partant
			 * de la position actuelle. Si un carrefour n'a plus de branches à
			 * visiter on le dépile. Une fois la cible trouvée il n'y a plusqu'à
			 * parcourir la pile à rebours en sélectionnant à chaque fois le cap
			 * qui a été utilisé.
			 * 
			 * Trois piles : une pour les carrefours, une pour les branches
			 * qu'il reste à parcourir et une dernière pour savoir où l'on va et
			 * d'où l'on vient.
			 * 
			 * Parcours de graph certainement pas optimal, ne cherche pas plus
			 * cours chemin dans un éventuel (mais fortement non recommandé)
			 * graphe avec cycles.
			 */

			Diary.logln("Calcul d'un itinéraire entre " + nodeFrom.toString()
					+ " et " + nodeTo.toString());

			if (nodeFrom.equals(nodeTo)) {
				Diary.logln("Sans bouger, on est déjà arrivé.");
				return;
			}

			// Pile des carrefours explorés, contient Junction
			Stack nodeStack = new Stack();
			// Pile des branches explorées, contient Cape[]
			Stack branchesStack = new Stack();

			// Carrefour étudié en ce moment
			Junction currentNode;
			// Ses branches explorées
			Cape[] branches;
			// Prochain carrefour à parcourir
			Junction nextNode = null;

			// Le dernier cap sélectionné
			Cape lastCape = null;

			// Commençons par le commencement
			nodeStack.push(nodeFrom);
			branchesStack.push(nodeFrom.getKnownBranches());

			// Tant que la pile des carrefour n'est pas vide (auquel cas on
			// aurait rient trouvé) ou tant que le carrefour qu'on vient
			// d'empiler est différent de notre objectif
			while (!nodeStack.isEmpty()
					&& (nextNode == null || !nextNode.equals(nodeTo))) {
				currentNode = (Junction) nodeStack.peek();
				// Diary.logln("node :" + currentNode);
				branches = (Cape[]) branchesStack.peek();
				// Diary.logln("nombre de branches : " + branches.length);
				// On parcours les cap existant tant qu'on ne trouve pas
				// prochain carrefour
				for (int i = 0; i < branches.length; i++) {
					// Diary.logln("branche n° : " + i);
					// Un cap à visiter
					if (branches[i] != null) {
						// Diary.logln("branche :" + branches[i]);
						// Si on ne retourne pas sur nos pas
						if (lastCape == null
								|| branches[i] != lastCape.opposite()) {
							// Diary.logln("on s'y intéresse");
							// Sélectionne carrefour au bout du cap
							nextNode = currentNode.getJunction(branches[i]);
							// Enregistre le cap
							currentNode.selectCape(branches[i]);
							// idem, mais pour savoir d'où on vient lors de la
							// prochaine itération
							lastCape = branches[i];
							// Marque la branche
							branches[i] = null;
							// Ajoute ce qu'il faut aux piles
							nodeStack.push(nextNode);
							branchesStack.push(nextNode.getKnownBranches());
							// Un carrefour à tester : fini pour cette boucle
							break;
						} else {
							// Diary.logln("On ne s'intéresse pas à cette branche");
							nextNode = null;
						}
					}
				}
				// Si ce carrefour ne mène nulle part, on l'enlève avant tour
				// suivant
				if (nextNode == null) {
					// Diary.logln("carrefour ne mène nulle part, enlève");
					// On lui enlève le cap
					currentNode.selectCape(null);
					// Et donc on partira vers une nouvelle direction
					lastCape = null;
					nodeStack.pop();
					branchesStack.pop();
				}
			}
			// Rien trouvé, mériterait exception
			if (nodeStack.isEmpty())
				Diary.logln("Erreur : aucun itinéraire n'a pu être calculé");
			// Sinon on peut afficher l'itinéraire, les cap on été marqué au fur
			// et à mesure.
			else {
				Diary.logln("Itinéraire calculé : ");
				nextNode = nodeFrom;
				while (nextNode.getSelectedCape() != null) {
					Diary.logln(nextNode.toString() + "->"
							+ nextNode.selectedCape);
					nextNode = nextNode.getSelectedJunction();
				}

				Diary.logln(nodeTo.toString() + " -> arrivé !");
			}
		}
	}

	private Map _map;

	public Cartographer(OrganEyeColor seeColor) {
		this(seeColor, true);
	}

	/**
	 * Avec cette version du constructeur, on peut indiquer à Cartographer s'il
	 * doit se préoccuper ou non des cap Est et Ouest (utile pour créer une
	 * version simplifié du parcours).
	 * 
	 * @param seeColor
	 *            organe visionnant la ligne au sol
	 * @param watchSides
	 *            true pour que le robot explore les Cape.WEST et Cape.EAST
	 */
	public Cartographer(OrganEyeColor seeColor, boolean watchSides) {
		_map = new Map();
		_seeColor = seeColor;
		_watchSides = watchSides;
	}

	/**
	 * Étant-donné qu'on se réfère au Nord et qu'on cherche le plus court
	 * chemin, un angle est compris entre ]-180,180]. Exemples : -180 -> 180,
	 * 270 -> -90.
	 * 
	 * NB: l'algo est déjà moche, mais en plus si on veut l'utiliser ailleurs il
	 * faudra bien y regarder à deux fois. Par exemple en java -270 % 180 = -90
	 * quand en python c'est directement égal au désiré 90.
	 * 
	 * @param angle
	 *            l'angle à normaliser
	 * @return valeur de l'angle après simplification
	 */
	public static float normalizeAngle(float angle) {
		// J'ai la désagréable sensation de calculer très stupidement
		angle %= 360;
		// On est maintenant dans ]-360,360[
		if (angle <= 180) {
			if (angle > -180) {
				// Il était déjà parfaitement dans les normes
				return angle;
			} else {
				if (angle == -180)
					return 180;
				// ]-360,-180[
				return (angle + 360) % 180;
			}
		}
		// ] 180 , 360 [
		return (angle % 180) - 180;
	}

	/**
	 * Factorise la découverte d'une branche initiée dans discoverJunction()
	 * 
	 * @param jun
	 *            le carrefour qu'on explore
	 * @param cap
	 *            le cap où l'on se trouve
	 */
	private void tasteBranche(Junction jun, Cape cap) {
		boolean hasBranch = false;
		// On laisse le robot souffler un peu le temps que Thalamus fasse un
		// nouveau tour des organes et que OrganEyeColor daigne répondre.
		Motion.haveABreak(TIME_ON_EACH_BRANCH);
		// Si c'est autre chose que du blanc alors c'est un chemin valide
		if (_seeColor.getSensing() != 1)
			hasBranch = true;
		jun.setBranche(cap, hasBranch);
	}

	/**
	 * Le robot se trouve sur une balise rouge. C'est un nouveau carrefour dont
	 * il va explorer les différentes branches qui en partent. Sera ajouté à la
	 * carte par onJunction(). Connexion dans les deux sens établie avec le
	 * précédent carrefour s'il existe.
	 * 
	 * @return le nouveau carrefour
	 */
	private Junction discoverJunction() {
		// On ne découvre que si l'on flâne
		if (_currentRoute != Route.EXPLORE)
			throw new IllegalStateException(
					"Découverte d'un carrefour alors que la stratégie est fixé à "
							+ _currentRoute + " au lieu de " + Route.EXPLORE);

		// Ces tâtonnements sont pour rire
		_seeColor.setPassive(true);

		// Le nouveau carrefour sur lequel on se trouve
		Junction node;
		// Le cap où l'on va
		Cape cap;
		// Le précédent carrefour
		Junction lastJunction = _map.getLastJunction();
		// Si la carte n'a pas renvoyé de dernier carrefour c'est que le robot
		// vient de rencontrer sa niche et qu'il apparaît du Sud.
		if (lastJunction == null) {
			node = new Junction(JunctionType.KENNEL);
			Diary.logln("C'est ma niche !");
			_hasDiscoveredKennel = true;
			cap = Cape.NORTH;
		} else {
			node = new Junction(JunctionType.NODE);
			// Dans le cas contraire on sait de quel cap on vient et donc où
			// l'on va
			cap = lastJunction.selectedCape;
			Cape comingFromCap = cap.opposite();
			// Peut le relier à présent carrefour
			lastJunction.setJunction(cap, node);
			// Et vis-versa
			node.setJunction(comingFromCap, lastJunction);
		}

		// On veut se déplacer d'une certaine distance pour sortir du carrefour
		float moveDistance = 75;

		/*
		 * TODO: utiliser ce nouveau Motion.rotateShortest dont je suis si fier.
		 * Après-tout si je ne suis pas là pour lui, qui va lui tendre la main,
		 * ne serait-ce que lui accorder l'aumone d'un regard ?
		 */

		// Nouveau carrefour, on remet dans l'axe et teste devant
		Motion.changeCapeAngle(cap.getAngle());
		Motion.rotateFromCape(0);
		Motion.travel(moveDistance);
		tasteBranche(node, cap);

		// retour
		Motion.travel(-moveDistance);

		// Si on ne vient de nulle part, il faut aussi regarder ce qui se passe
		// derrière
		if (node.getType() == JunctionType.KENNEL) {
			// en arrière
			Motion.travel(-moveDistance);
			cap = cap.opposite();
			tasteBranche(node, cap);
			// retour
			Motion.travel(moveDistance);
			// revient à cap initial pour pouvoir correctement sélectionner
			// adjacent
			cap = cap.opposite();
		}

		// Par défaut on regarde les côtés
		if (_watchSides) {
			// Côté adjacent
			cap = cap.adjacent();
			Motion.changeCapeAngle(cap.getAngle());
			Motion.rotateFromCape(0);
			Motion.travel(moveDistance);
			tasteBranche(node, cap);

			// retour
			Motion.travel(-moveDistance);

			// De l'autre côté : on ne tourne pas, on recule
			cap = cap.opposite();
			Motion.travel(-moveDistance);
			tasteBranche(node, cap);

			// retour
			Motion.travel(moveDistance);
		}
		// Sans visiter on définit les branches EAST/WEST comme inexistantes
		else {
			node.setBranche(cap.adjacent(), false);
			node.setBranche(cap.adjacent().opposite(), false);
		}

		// NB: Laisse la suite choisir le cap et tourner comme il convient robot

		Diary.logln("fin exploration");

		// Ne pas oublier de le remettre actif sous peine de désagréments
		_seeColor.setPassive(false);

		return node;
	}

	private Junction discoverRoom(String name) {
		// On ne découvre que si l'on flâne
		if (_currentRoute != Route.EXPLORE)
			throw new IllegalStateException(
					"Découverte d'une salle alors que la stratégie est fixé à "
							+ _currentRoute + " au lieu de " + Route.EXPLORE);

		// Le nouveau carrefour sur lequel on se trouve
		Junction node = new Junction(JunctionType.ROOM, name);
		// Le précédent carrefour
		Junction lastJunction = _map.getLastJunction();
		// Pas possible d'avoir été parachuté de la sorte
		if (lastJunction == null)
			throw new IllegalStateException(
					"Problème : vient de nulle part et salle directement");
		else {
			// le cap d'où l'on vient
			Cape cap = lastJunction.selectedCape;
			// Relie précédent carrefour et pièce
			lastJunction.setJunction(cap, node);
			node.setJunction(cap.opposite(), lastJunction);

			// Une salle est un cul-de-sac, renseigne comme il se doit les
			// autres caps.
			node.setBranche(cap, false);
			node.setBranche(cap.adjacent(), false);
			node.setBranche(cap.adjacent().opposite(), false);
		}
		return node;
	}

	/**
	 * Le robot vient d'arriver sur un carrefour. Va l'explorer s'il est nouveau
	 * ou bien directement continuer son chemin
	 */
	public void onJunction() {
		// Le carrefour sur lequel il est sensé se trouver
		Junction currentJunction = _map.getNextJunction();
		// Si la carte n'a pas indiqué quel est le prochain carrefour c'est
		// qu'il est inconnu
		if (currentJunction == null) {
			Diary.logln("Nouveau carrefour");
			currentJunction = discoverJunction();
		} else
			Diary.logln("Carrefour connu");
		// On indique à la carte notre position
		_map.setJunction(currentJunction);
	}

	/**
	 * Le robot vient d'arriver sur une salle. La crée si elle est nouvelle ou
	 * bien continue son chemin
	 */
	public void onRoom(String roomName) {
		Diary.logln("(Cartographer) Sur salle : " + roomName);

		// Le carrefour sur lequel il est sensé se trouver
		Junction currentJunction = _map.getNextJunction();
		// Si la carte n'a pas indiqué quel est le prochain carrefour c'est
		// qu'il est inconnu
		if (currentJunction == null) {
			Diary.logln("Nouvelle salle");
			currentJunction = discoverRoom(roomName);
			_map.setJunction(currentJunction);
		} else {
			// Sinon deux possibilité : mauvaise salle ou bien c'est la bonne et
			// on peut tranquillement continuer
			if (!currentJunction.getId().equals(roomName)) {
				throw new IllegalStateException(
						"Problème ! Ce n'est pas la bonne salle : "
								+ currentJunction.getId() + " attendu !");
			} else {
				Diary.logln("Salle connue");
				// On indique à la carte notre position
				_map.setJunction(currentJunction);
			}
		}
	}

	/**
	 * Utilisé par ReflexRotate pour savoir vers où le robot doit se tourner.
	 * 
	 * @return Position de la branche choisie
	 */
	public Cape getNextCape() {
		return _map.getNextCape();
	}

	/**
	 * On signale à Cartographer qu'il faut tracer un nouvel itinéraire. Utiler
	 * setRoute(route,name) pour une salle (il faut la nommer).
	 * 
	 * @param route
	 *            où doit se diriger le robot maintenant
	 */
	public void setRoute(Route route) {
		if (route == Route.ROOM)
			throw new IllegalStateException(
					"Il faut fournir le nom de la salle");
		_currentRoute = route;
		_map.computeRoute();
	}

	/**
	 * On signale à cartographer qu'il faut se rendre dans cette pièce
	 * particulière
	 * 
	 * @param route
	 *            Route.ROOM
	 * @param roomName
	 *            nom de la salle à retrouver, de la forme "couleur-couleur"
	 */
	public void setRoute(Route route, String roomName) {
		_currentRoute = route;
		_currentRouteRoomName = roomName;
		_map.computeRoute();
	}

	/**
	 * Map a signalé que la destination avait été atteinte ou bien ou contraire
	 * qu'un nouvel itinéraire a été commandé via computeRoute()
	 * 
	 * @param toggle
	 *            true si robot a atteint son but, false sinon
	 */
	private void setDestinationReached(boolean toggle) {
		_destinationReached = toggle;
	}

	/**
	 * Est-ce que le robot a atteint son but ? Utilisé par OrganCartographer
	 * pour
	 * 
	 * @return true si destination atteinte, false sinon
	 */
	public boolean isDestinationReached() {
		return _destinationReached;
	}

	/**
	 * Utilisé par OrganCartographer pour déterminer quelle type d'exploration
	 * est terminée.
	 * 
	 * @return le type du dernier carrefour rencontré
	 */
	public JunctionType getLastJunctionType() {
		return _map.lastJunction.getType();
	}

	/**
	 * Utilisé par OrganCartographer pour déterminer quelle type d'exploration
	 * est terminée
	 * 
	 * @return le type d'itinéraire en cours
	 */
	public Route getRoute() {
		return _currentRoute;
	}

	/**
	 * Est-ce qu'il reste des carrefours à explorer ? Utilisé par
	 * BehaviorExplore. Calculé à chaque découverte de salle.
	 * 
	 * @return true si il reste des carrefours à explorer OU que ça n'a pas été
	 *         encore déterminé, false si plus rien à défricher
	 */
	public boolean hasWildJunctions() {
		return _map.nbWildJunctions != 0;
	}

	/**
	 * BehaviorGoStraight veut savoir si doit avancer même sans direction
	 * indiqué pour essayer de tomber sur la niche
	 */
	public boolean hasDiscoveredKennel() {
		return _hasDiscoveredKennel;
	}
}
